/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates.commands;

import edu.wpi.first.wpilibj.GenericHID;
import edu.wpi.first.wpilibj.smartdashboard.SmartDashboard;
import edu.wpi.first.wpilibj.templates.OI;
import edu.wpi.first.wpilibj.templates.RobotState;

/**
 *
 * @author matt
 */
public class manualDrive extends CommandBase {

    RobotState robotState = RobotState.getInstance();
    private boolean m_lastSwerveButton = false;
    private boolean m_currSwerveButton = false;

    public manualDrive() {
        // Use requires() here to declare subsystem dependencies
        requires(leftDriveSubsystem);
        requires(rightDriveSubsystem);
        System.out.println("manual Drive constructed");
    }

    // Called just before this Command runs the first time
    protected void initialize() {
    }

    // Called repeatedly when this Command is scheduled to run
    protected void execute() {
        double speedScale = 0.75;

        // if the left top button is pressed, go slow
        if (oi.getDriverController().getButtonLB()) {
            speedScale = 0.375;
        } // if the left trigger is pressed, go fast
        else if (oi.getDriverController().getButtonLeftTrigger()) {
            speedScale = 1.0;
        }
        
        
        // Primary Driver Controller
        double leftSpeed = speedScale * oi.getDriverController().getLeftY();
        double rightSpeed = speedScale * oi.getDriverController().getRightY();
        
        // Secondary CoDriver Controller
        oi.getCoDriverController().getRightX();
        oi.getCoDriverController().getRightY();
        
        double delta = 0.3 * oi.getCoDriverController().getRightX();
        double common = 0.3 * oi.getCoDriverController().getRightY();
        
        leftSpeed = leftSpeed + common + delta;
        leftSpeed = leftSpeed + common - delta;
        //leftSpeed = leftSpeed + 0.3 * oi.getCoDriverController().getLeftY();
        //rightSpeed = rightSpeed + 0.3 * oi.getCoDriverController().getRightY();

        SmartDashboard.putDouble("DriveSpeedLeft", speedScale * leftSpeed);
        SmartDashboard.putDouble("DriveSpeedRight", speedScale * rightSpeed);
        
//        System.out.print(" Swerve # " + swerveSubsystem.getCommandedPosition() + " @ " + swerveSubsystem.getPosition() );
//        System.out.print(" - Lifter LF@ " + leftForeLifterSubsystem.getPosition() );
//        System.out.print("   Lifter LR@ " + leftRearLifterSubsystem.getPosition() );
//        System.out.print("   Lifter RF@ " + rightForeLifterSubsystem.getPosition() );
//        System.out.print("   Lifter RR@ " + rightRearLifterSubsystem.getPosition() );
//        System.out.println(" ");
        RobotState.RobotDirection currentDirection = robotState.getDirection();
           
        if (RobotState.RobotDirection.kForward == currentDirection) {
            leftDriveSubsystem.drive(leftSpeed);
            rightDriveSubsystem.drive(rightSpeed);
            //System.out.print("FW");
        } else if (RobotState.RobotDirection.kLeft == currentDirection) {
            leftDriveSubsystem.drive(leftSpeed);
            rightDriveSubsystem.drive(rightSpeed);
            //System.out.print("LT");
        } else if (RobotState.RobotDirection.kReverse == currentDirection) {
            leftDriveSubsystem.drive(rightSpeed);
            rightDriveSubsystem.drive(leftSpeed);
            //System.out.print("RV");
        } else if (RobotState.RobotDirection.kRight == currentDirection) {
            leftDriveSubsystem.drive(rightSpeed);
            rightDriveSubsystem.drive(leftSpeed);
            //System.out.print("RT");
        } else {
            System.out.println("Manual Drive - Bad Robot Direction State");
        }
        //System.out.println(":" + speedScale + "-- " + leftSpeed + " _ " + rightSpeed);

    }

    // Make this return true when this Command no longer needs to run execute()
    protected boolean isFinished() {
        return false;
    }

    // Called once after isFinished returns true
    protected void end() {
    }

    // Called when another command which requires one or more of the same
    // subsystems is scheduled to run
    protected void interrupted() {
    }
}
